#include <Arduino.h>

#include "CCommand.h"

CCommand::CCommand(unsigned char rawByte1, unsigned char rawByte2, unsigned char rawByte3, unsigned char rawByte4, unsigned char rawByte5)
: mCommandType(toCommand(rawByte1))
, mValue32(0)
, mValue8_1(rawByte2)
, mValue8_2(rawByte3)
, mValue8_3(rawByte4)
, mValue8_4(rawByte5)
{
   mValue32 += rawByte2;
   mValue32 = mValue32 << 8;
   mValue32 += rawByte3;
   mValue32 = mValue32 << 8;
   mValue32 += rawByte4;
   mValue32 = mValue32 << 8;
   mValue32 += rawByte5;
}

CCommand::CCommand(eCommand cmd, long value)
: mCommandType(cmd)
, mValue32(value)
, mValue8_1(0)
, mValue8_2(0)
, mValue8_3(0)
, mValue8_4(0)
{
   long temp = value >> 8;
   temp = temp << 8;
   mValue8_4 = value - temp;
   value = value >> 8;
   
   temp = value >> 8;
   temp = temp << 8;
   mValue8_3 = value - temp;
   value = value >> 8;
   
   temp = value >> 8;
   temp = temp << 8;
   mValue8_2 = value - temp;
   value = value >> 8;
   
   temp = value >> 8;
   temp = temp << 8;
   mValue8_1 = value - temp;
}

CCommand::CCommand(eCommand cmd, unsigned char value1, unsigned char value2, unsigned char value3, unsigned char value4)
: mCommandType(cmd)
, mValue32(0)
, mValue8_1(value1)
, mValue8_2(value2)
, mValue8_3(value3)
, mValue8_4(value4)
{
   mValue32 += value1;
   mValue32 = mValue32 << 8;
   mValue32 += value2;
   mValue32 = mValue32 << 8;
   mValue32 += value3;
   mValue32 = mValue32 << 8;
   mValue32 += value4;
}

void CCommand::send()
{
   Serial.write(mCommandType);
   Serial.write(mValue8_1);
   Serial.write(mValue8_2);
   Serial.write(mValue8_3);
   Serial.write(mValue8_4);
}
